// 获取距离的函数 (单位: 厘米)
float getDistance() {
  // 确保Trig引脚为低电平起始状态
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  // 产生一个10微秒的高脉冲触发测距
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  // 读取Echo引脚的高电平持续时间 (单位: 微秒)
  // 超时时间约为 max_distance * 2 * 29.1 us/cm
  long duration = pulseIn(ECHO_PIN, HIGH, MAX_DISTANCE * 58); 
  // 计算距离 (距离 = (时间 * 声速) / 2)
  float distance = duration * SOUND_SPEED / 2;
  return distance;
}
void sub_ds(){
  float distance = getDistance(); // 获取前方距离

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // 简单避障逻辑
  if (distance < 20) { // 如果距离小于20厘米
    stopCar();         // 先停止
    delay(200);
    moveBackward(150); // 后退
    delay(500);
    stopCar();
    delay(200);

    // 随机向左或向右转
    if (random(0, 2) == 0) { // 生成0或1的随机数
      turnLeft(150);
    } else {
      turnRight(150);
    }
    delay(600); // 转向持续时间
    stopCar();
    delay(200);
  } else {
    moveForward(180); // 没有障碍物，继续前进
  }
  delay(100); // 每次循环之间稍作延迟，避免测距过于频繁
  
}
